/*Goerttzel算法*/

/*求取一段周期波形中，某一特定频率的初相位*/
#define PI                  3.1415926535

#define SIGNAL_FREQ         5000        //signal frequency
#define SAMPLE_POINT_SIZE   192
#define SAMPLE_RATE         160000

#define K_COEF      (SIGNAL_FREQ*SAMPLE_POINT_SIZE/SAMPLE_RATE) //Goertzel coefficent - MUST be integer, or quality fall

#define SCALING     ((FP32)SAMPLE_POINT_SIZE / 2.0)     //scaling the data
#define OMEGA       ((2.0 * PI * K_COEF) / SAMPLE_POINT_SIZE)

#define INT_COEF    1024    //sin/cos Amp//正余弦数组幅值

FP32 Goertzel(INT16U* data)
{
    volatile INT16U ii;
    volatile INT64S real = 0.0;
    volatile INT64S imag = 0.0;
    volatile FP32 Phase;
    
    real = 0;
    imag = 0;
    
    for(ii = 0 ; ii < SAMPLE_POINT_SIZE ; ii++)
    {
        real += ((INT64S)(g_SinBuff[ii]) * (INT64S)(data[ii]));
        imag += ((INT64S)(g_CosBuff[ii]) * (INT64S)(data[ii]));
    }

    real = real / (INT64S)SCALING;
    imag = imag / (INT64S)SCALING;

    Phase = GetPhase((FP32)real, (FP32)imag);

    return Phase;
}

void InitGoertzel(void)
{
    INT16U ii;
    
    for(ii = 0 ; ii < SAMPLE_POINT_SIZE ; ii++)
    {
      g_SinBuff[ii] = (INT32S)(sin(ii * OMEGA) * INT_COEF);
      g_CosBuff[ii] = (INT32S)(cos(ii * OMEGA) * INT_COEF);
    }
}